#ifndef CONVERTTOSTANDARDFORM_HPP
#define CONVERTTOSTANDARDFORM_HPP

#include <limits>
#include <vector>
// TODO
#include <iostream>

#include <boost/tuple/tuple.hpp>
#include <boost/assert.hpp>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SPQRSupport>

#include <LinearProgramming/Utility.hpp>

namespace green {

/*!
 * Converts input data to standard form, TODO Range is not yet supported
 *
 * TODO Make member function as non member functions for better abstraction
 * TODO Had to implement lot of presolving techniques
 *
 * TODO All changes to X variables should be tracked to bring back actual
* values.
 */
template <class Parser>
class Presolve {
 public:
  /*!
   * \brief ConvertToStandardForm
   * \param parser Parser instance which converts inpput format into C++ DS
   * Constructor reads the data and converts to standard form
   */
  Presolve(const Parser& parser) : parser(parser) {
    // TODO Check if move constructor is invoked here very crucial
    // Or some kind optimization by Eigen which we do not yet know check
    // check...
    // Size of Vectors and Matrix are obtained from right hand side parser
    // functions, so no need to define size in this class, as we are not
    // building
    // them here
    // FIXME Use method reserve for better performance, anticipate all changes
    objective = this->parser.getObjectiveCoefficients();
    rhs = this->parser.getRHS();

    constraints = this->parser.getConstraints();

    rowSense = parser.getRowSense();
  }

  void standardize() {

    // First thing process columns
    // Have to maintain Original index of columns in constraints as we are going
    // to add new columns in between
    // ----Do we have to add in between? No.. We can add at the end so avoiding
    // storage of index and swapping the data Yahooooooo
    // ----It would increase expense of calculating final values...It might
    // anyways
    // ------So picture out all possibilities and procede

    // Start modifying given problem, private method might look abstract as they
    // do modify class members (like modifying global variables),
    // TODO we do not know which method modified which class member... had to
    // find a better way

    standardizeColumns();
    standardizeRowSense();

    convertTofullRowRank();
  }

  Eigen::SparseMatrix<double> getConstraints() const { return constraints; }

  Eigen::SparseVector<double> getObjective() const { return objective; }

  Eigen::SparseVector<double> getRHS() const { return rhs; }

 private:

  enum class ColumnType {
    STANDARD, INTERVAL, UPPER_BOUND, LOWER_BOUND, FIXED, FREE
  };

  using LowerUpperTuple = boost::tuple<const double&, const double&>;

  const Parser& parser;

  Eigen::SparseVector<double> objective;
  Eigen::SparseVector<double> rhs;

  Eigen::SparseMatrix<double> constraints;

  // Enhance rowSense as new rows are added after column standardization
  std::vector<typename Parser::RowSense> rowSense;

  /*!
   * \brief standardizeRowSense
   *
   * RowSense values are enumerations from Praser template parmeter
   *
   * RowSense::LESS_THAN
   *  add new positive column in constriants, sample vector [0,0,0,1,0,0]
   *  where non zero value 1 at row index of interest
   * RowSense::GREATER_THAN
   *  add new negative column in constraints, sample vector [0,0,0,-1,0,0]
   * RowSense::EQUAL_TO
   *  nonthing to do
   *
   * All new columns are added at the end
   * 1. Count number of less_than and greater_than rowSense values, this
   *    information is used to resize the constriants matrix, rather than
   *    resizing constraint matrix in each iteration. (Equal_to is ignored,
   *    as it already satisfies final form)
   * 2. Based on rowSense trait, add negative column or positive column
   *
   * Added column indicates new variable added to the system, but this new
   * variable has 0 coefficient in objective function, so no need to worry,
   * objective function is same
   */
  void standardizeRowSense() {
    int numberOfLessGreater = 0;
    int filledColumns = constraints.cols();

    for_each(rowSense.cbegin(), rowSense.cend(),
             [&](typename Parser::RowSense rowSenseType) {
      if (rowSenseType == Parser::RowSense::LESS_THAN ||
          rowSenseType == Parser::RowSense::GREATER_THAN) {
        ++numberOfLessGreater;
      }
    });

    constraints.conservativeResize(constraints.rows(),
                                   constraints.cols() + numberOfLessGreater);

    int index = 0;
    for_each(rowSense.cbegin(), rowSense.cend(),
             [&](typename Parser::RowSense rowSenseType) {

      if (rowSenseType == Parser::RowSense::LESS_THAN) {
        constraints.insert(index, filledColumns) = 1;
        ++filledColumns;
      } else if (rowSenseType == Parser::RowSense::GREATER_THAN) {
        constraints.insert(index, filledColumns) = -1;
        ++filledColumns;
      }

      ++index;
    });
  }

  void standardizeColumns() {

    std::vector<double> upperBound{parser.getColumnUpperBounds()};
    std::vector<double> lowerBound{parser.getColumnLowerBounds()};

    // TODO Is there a better way to get index of Iterator
    int index = 0;
    for (const auto&& lowerUpperBounds :
         green::internal::zip_range(lowerBound, upperBound)) {

      switch (getColumnType(lowerUpperBounds)) {
        case ColumnType::INTERVAL:
          standardizeColumnInterval(lowerUpperBounds, index);
          break;
        case ColumnType::FREE:
          standardizeColumnFree(index);
          break;
        case ColumnType::LOWER_BOUND:
          standardizeColumnLowerBound(lowerUpperBounds, index);
          break;
        case ColumnType::UPPER_BOUND:
          standardizeColumnUpperBound(lowerUpperBounds, index);
          break;
        case ColumnType::STANDARD:
          // Yeey do nothing
          break;
        default:
          break;
      }

      ++index;
    }
  }

  /*!
   * \brief standardizeColumnInterval
   * \param lowerUpperTuple
   * \param index
   *  Standardizes interval column
   *  L <= X <= U to
   *  Column X is changed to X = Y + L;
   *  New Row Y <= U - L
   *  s.t. Y >= 0
   *
   *  Enhance rowSense vector as new rows are added here
   *
   */
  void standardizeColumnInterval(const LowerUpperTuple& lowerUpperTuple,
                                 const int index) {
    // 1. Change RHS
    // b = b - L * A.col(index)
    rhs = rhs - lowerUpperTuple.get<0>() * constraints.col(index);
    // 2. Add new Row
    //    Add new row in rhs
    rhs.conservativeResize(rhs.rows() + 1);
    rhs.insert(rhs.rows() - 1) =
        lowerUpperTuple.get<1>() - lowerUpperTuple.get<0>();
    //    Add new row in constraints
    constraints.conservativeResize(constraints.rows() + 1, constraints.cols());
    constraints.insert(constraints.rows() - 1, index) = 1;

    rowSense.emplace_back(Parser::RowSense::LESS_THAN);
  }

  /*!
   * \brief standardizeColumnFree
   * \param lowerUpperTuple
   * \param index
   *
   * Standardize Free column -Inf <= X <= +Inf
   * Column X is changed to Y+ - Y- (YPositive - YNegative)
   * New Column is appended at the end, existing column is not changed during
   * substitution
   *
   * TODO Store information about substitution?
   */
  void standardizeColumnFree(const int index) {
    // 1. Add new column (Negative of original column)
    constraints.conservativeResize(constraints.rows(), constraints.cols() + 1);

    constraints.col(constraints.cols() - 1) =
        constraints.middleCols<1>(index) * -1;

    // 2. Add new row to objective (Negative of original row)
    objective.conservativeResize(objective.rows() + 1);
    objective.insert(objective.rows() - 1) = objective.coeff(index) * -1;
  }

  /*!
   * \brief standardizeColumnLowerBound
   * \param index
   *
   * Column X has lower bound
   * L <= X <= Inf
   * To standardize substitute X = Y + L
   */
  void standardizeColumnLowerBound(const LowerUpperTuple& lowerUpperTuple,
                                   const int index) {
    // 1. Change RHS
    // b = b - L * A.col(index)
    rhs = rhs - lowerUpperTuple.get<0>() * constraints.col(index);
    // 2. No need to change X column
  }

  /*!
   * \brief standardizeColumnUpperBound
   * \param lowerUpperTuple
   * \param index
   *
   * Column X has upper bound
   * -Inf <= X <= U
   * To standardize substitute X = -Y + U
   */
  void standardizeColumnUpperBound(const LowerUpperTuple& lowerUpperTuple,
                                   const int index) {
    // 1. Change RHS
    // b = b - U * A.col(index)
    rhs = rhs - lowerUpperTuple.get<1>() * constraints.col(index);
    // 2. Change sign of X in constraints
    constraints.col(index) = constraints.col(index) * -1;
    // 3. Change sign of X in objective
    objective.coeffRef(index) = objective.coeffRef(index) * -1;
  }

  /*!
   * \brief getColumnType
   * \param lowerUpperTuple
   * \return is based on following conditions
   * L(lower), U(upper), V(value) are constants
   *
   * 0    to Inf -- standard
   * -Inf to U   -- Upper Bound
   * L    to Inf -- Lower Bound
   * -Inf to Inf -- Free
   * 0    to U   -- Interval
   * L    to 0   -- Interval
   * L    to U   -- Interval
   * -Inf to 0   -- Negative standard, Same as Upper bound where U = 0
   *      == V   -- Fixed (Value is solved, move to rhs )
   *
   */
  ColumnType getColumnType(const LowerUpperTuple& lowerUpperTuple) const {
    // TODO Had to read about How auto is determined for const references, is
    // constant and reference preserved or only type is deduced?
    const auto& lowerBound = lowerUpperTuple.get<0>();
    const auto& upperBound = lowerUpperTuple.get<1>();
    // numerical limits lowest and max are used as -inf and inf
    const auto& min = std::numeric_limits<double>::lowest();
    const auto& max = std::numeric_limits<double>::max();

    if (lowerBound > min && upperBound < max) {
      return ColumnType::INTERVAL;
    } else if (lowerBound == min && upperBound == max) {
      return ColumnType::FREE;
    } else if (lowerBound == min && upperBound < max) {
      return ColumnType::UPPER_BOUND;
    } else if (lowerBound > min && upperBound == max) {
      return ColumnType::LOWER_BOUND;
    } else if (lowerBound == 0 && upperBound == max) {
      return ColumnType::STANDARD;
    }
    // If not above it has to be fixed
    // If its fixed, value is already known, variable can be removed from
    // constraints & objective function,
    // this changes RHS, as we move known variable from LHS to RHS
    BOOST_ASSERT_MSG(1 == 2,
                     "Not yet, but who would define their problem like this?");
    return ColumnType::FIXED;
  }

  /*!
   * \brief convertTofullRowRank
   * Full row rank is required for linear programming, as all proofs are
   * considered with full row rank of a constrained matrix
   *
   * Determination of full row rank
   * 1. Using QR (SPQR) from suitesparse as it has more chance of getting
   *    parallelized (in documentaion), Eigen has also QR implemented I do not
   *    know the performace comparision between Eigen QR and suitesparse QR
   *    (TODO Get the benchmarks for both functions)
   * 2. QR is used to find the column rank of matrix (I did not find any way to
   *    do it to get for row rank ).
   * 3. First matrix is transposed to get rows to columns (we should copy
   *    constraint
   *    matrix to avoid changing original ), the QR is called to get rank and
   *    permutation matrix.
   * 4. Permuation is multiplied from left hand side to bring up all independent
   *    rows to top, and then Rank is used to resize (shrink) the row size of
   *    matrix, which will also remove all dependent rows.
   * 5. Same is done for RHS
   */
  void convertTofullRowRank() {

    Eigen::SPQR<Eigen::SparseMatrix<double>> qrSolver;
    // TODO Use constants from common header
    qrSolver.setPivotThreshold(1.0e-10);
    qrSolver.compute(constraints.transpose());

    // TODO When I use same matrix on both side resultant matrix is full of
    // zeros, this is eigen way of doing, have to find reason and better way to
    // do it
    Eigen::SparseMatrix<double> temporarySM(constraints);
    constraints = qrSolver.colsPermutation() * temporarySM;

    constraints.conservativeResize(qrSolver.rank(), constraints.cols());

    // TODO Operator= is not working for SparseVectors ( rhs = permuation * rhs)
    // As a workaround SparseVector is first converted to SparseMatrix, then
    // permuted and assigned back to rhs
    // Error: No matching function found
    Eigen::SparseMatrix<double> rhsMatrix(rhs.rows(), rhs.cols());

    rhsMatrix = qrSolver.colsPermutation() * rhs;
    rhsMatrix.conservativeResize(qrSolver.rank(), rhs.cols());
    // Finally assign back to rhs, FIXME Had to find a way out
    rhs = rhsMatrix;
  }
};
}
#endif  // CONVERTTOSTANDARDFORM_HPP
